#include <stdio.h>

#include "aos/hal/can.h"
#include "board.h"

#include "ulog/ulog.h"
#include "canbus.h"
#include "lidar.h"
#include "aos/cli.h"
#include "car.h"

extern int upping(int val);
extern void eta_opt(int val);


#define LOG_TAG "DOOR"

#define F_TF_DATA_TX_ID (0x188u)
#define PWM_ID0 (0x180u)
#define PWM_ID1 (0x181u)
#define PWM_ID2 (0x182u)

#define F_TF_DATA_RX_ID (0x88u)
#define CORRECT_VAL          (6)

#define DATA_SIZE_MAX (16)

static int log_flag_tf = 0;

typedef struct {
	uint8_t head_L;
	uint8_t head_h;
	uint8_t dist_l;
	uint8_t dist_h;
	uint8_t stren_l;
	uint8_t stren_h;
	uint8_t temp_l;
	uint8_t temp_h;
} tf_data_frm_t;

static canbus_dev_t dev_tf;
extern uint16_t b_dist;
extern uint16_t b_dist_l;

static uint8_t pwm_ctrl(uint32_t id, uint8_t *pbuf, uint8_t size)
{
	can_frame_t start_frame;
	canbus_dev_t *dev = &dev_tf;

	start_frame.header.id = id;
	start_frame.header.dlc = size;
	start_frame.header.rtr = 0;

	memcpy(start_frame.data, pbuf, size);

	lidar_tfmini_send(&start_frame);
	return size;
}

static void lidar_tf_cmd_routine(char *buf, int len, int argc, char **argv)
{
	uint8_t temp_buf[16] = {0}; 
	int arg1 = strtoul(argv[2], NULL, 10);
    int arg2 = strtoul(argv[3], NULL, 10);

	switch (*(argv[1]))
	{
	case 'p':
		aos_cli_printf("printf data log[%d]\r\n", arg2);
		log_flag_tf = arg1;
	   break;

	case 'f':
		aos_cli_printf("test send [%d %d]\r\n", arg1, arg2);
		switch(arg1){
			case 0:
				pwm_ctrl(arg2, "S1D100T", 8);
				//pwm_ctrl(arg2, "S1F00.0T", 8);
				break;
			case 1:
				pwm_ctrl(arg2, "S1F00.5T", 8);
				break;
			case 2:
				pwm_ctrl(arg2, "S1F01.0T", 8);
				break;
			case 3:
				pwm_ctrl(arg2, "S1F01.5T", 8);
				break;
			case 4:
				pwm_ctrl(arg2, "S1F02.0T", 8);
				break;
		}
		
	   break;
	case 'l':
		aos_cli_printf("left door [%d]\r\n", arg1);
		door_l(arg1);
		break;
	case 'r':
		aos_cli_printf("right door [%d]\r\n", arg1);
		door_r(arg1);
		break;
	case 'd':
		aos_cli_printf("door [%d]\r\n", arg1);
		if (arg1 == 3){
			door(2);
			aos_msleep(6000);
			door(0);
			aos_msleep(500);
			door(1);
			aos_msleep(6000);
			door(0);
		}else if (arg1 == 4){
			upping(400000);
			aos_msleep(3000);
			door_c(1);
			gpio_output(1, 14, 0);
			pwm_ctrl(PWM_ID1, "S2D050T", 7);
			aos_msleep(4300);
			//eta_opt(0);
			aos_msleep(5000);
			door_c(0);
			aos_msleep(500);
			door(2);
			aos_msleep(6000);
			door(0);
			aos_msleep(3000);
			pwm_ctrl(PWM_ID1, "S2D100T", 7);
			liftting(-350000);
			aos_msleep(8000);
			eta_opt(0);
			aos_msleep(3000);
			eta_opt(2);
			aos_msleep(1000);
			eta_opt(1);
			aos_msleep(3000);
			eta_opt(2);
			aos_msleep(1000);
			liftting(350000);
			aos_msleep(8000);
			gpio_output(1, 14, 1);
			pwm_ctrl(PWM_ID1, "S2D050T", 7);
			aos_msleep(3000);
			door(1);
			aos_msleep(6000);
			door(0);
			aos_msleep(500);
			door_c(2);
			aos_msleep(5000);
			//eta_opt(2);
			aos_msleep(4300);
			pwm_ctrl(PWM_ID1, "S2D100T", 7);
			door_c(0);
			aos_msleep(1000);
			upping(-400000);
			aos_msleep(3000);
		
		}else{
			door(arg1);
		}
		
		break;
	case 'c':
		aos_cli_printf("door [%d]\r\n", arg1);
		door_c(arg1);
		break;
	case 'g':
		aos_cli_printf("go [%d]\r\n", arg1);
		if (arg1 == 1){
			door(1);
			aos_msleep(6000);
			door(0);
			aos_msleep(500);
			door_c(2);
			aos_msleep(8300);
			door_c(0);
		}else if(arg1 == 2){
			door_c(1);
			aos_msleep(8300);
			door_c(0);
			aos_msleep(500);
			door(2);
			aos_msleep(6000);
			door(0);
		}else{
			door_c(0);
			aos_msleep(500);
			door(0);
		}
		
		break;
	case 't':
		aos_cli_printf("tui gan [%d %d]\r\n", arg1, arg2);
		if (arg1 == 1){
			gpio_output(1, 14, 1);
			pwm_ctrl(PWM_ID1, "S2D050T", 7);
		
		}else if (arg1 == 2){
			gpio_output(1, 14, 0);
			pwm_ctrl(PWM_ID1, "S2D050T", 7);
		}else{
			pwm_ctrl(PWM_ID1, "S2D100T", 7);
		}
		
	   break;
	case 'h':
		aos_cli_printf("hero moment\r\n");
		upping(300000);
		aos_msleep(2000);
		door_c(2);
		gpio_output(1, 14, 0);
		pwm_ctrl(PWM_ID1, "S2D050T", 7);
		aos_msleep(4300);
		eta_opt(0);
		aos_msleep(4000);
		door_c(0);
		aos_msleep(500);
		door(2);
		aos_msleep(6000);
		door(0);
		aos_msleep(1000);
		pwm_ctrl(PWM_ID1, "S2D100T", 7);
		liftting(-350000);
		aos_msleep(5000);
		eta_opt(2);
		front(1);
		aos_msleep(3000);
		front(0);
		aos_msleep(100);
		behind(1);
		aos_msleep(3000);
		behind(0);
		aos_msleep(100);
		left(1);
		aos_msleep(10000);
		left(0);
		aos_msleep(100);
		right(1);
		aos_msleep(10000);
		right(0);
		break;
	
	default:
	   aos_cli_printf("lidar Unknown cs %d\r\n", *(argv[1]));
	   break;
	}
}


static struct cli_command lidar_tf_cmd[] = {
    { "tf","control or get status of tf lidar", lidar_tf_cmd_routine },
};


static int lidar_tfmini_send(can_frame_t *frm) \
{
	if (NULL == frm ||
		NULL == dev_tf.bus ||
		NULL == dev_tf.bus->canbus_send) {
		return -1;
	}
	return dev_tf.bus->canbus_send(frm);
}

uint8_t lidar_tf_data_tx(uint8_t *pbuf, uint8_t size)
{
	can_frame_t start_frame;
	canbus_dev_t *dev = &dev_tf;

	start_frame.header.id = F_TF_DATA_TX_ID;
	start_frame.header.dlc = size;
	start_frame.header.rtr = 0;

	memcpy(start_frame.data, pbuf, size);

	lidar_tfmini_send(&start_frame);
	return size;
}

static void lidar_tfmini_dt_routine()
{
	int ret = -1;
	canbus_dev_t *dev = &dev_tf;

	dev->rx_buf = (uint8_t *)aos_malloc(DATA_SIZE_MAX);
	dev->rx_id = F_TF_DATA_RX_ID;
	ret = canbus_dev_register(dev);
	if (ret < 0) {
		aos_free(dev->rx_buf);
		LOGE(LOG_TAG, "dev lidarce30 register failed!");
		return ;
	}
	
	while(1){
		if (dev->sem_t == NULL) {
			aos_msleep(1000);
			continue;
		}
		aos_sem_wait((aos_sem_t *)&dev->sem_t, AOS_WAIT_FOREVER);
		tf_data_frm_t *pdata = (tf_data_frm_t *)dev->rx_buf;
		b_dist_l = b_dist;
		b_dist = (pdata->dist_l | (pdata->dist_h << 8)) + CORRECT_VAL;
		if (b_dist > 300){
			b_dist = 0;
		}
		if (pdata->head_h == 0x59 && log_flag_tf)
		{
			printf("tfdist %d\r\n", b_dist);
		}
		//aos_msleep(1000);
	}
}

int init_lidar_tfmini_plus()
{

	if (0 != aos_task_new("lidar_tfmini_dt", lidar_tfmini_dt_routine, NULL, CAN_DEMO_TASK_DEPTH)) {
		LOGE(LOG_TAG, "create lidar_tfmini_dt fail");
	}
	aos_cli_register_commands(&lidar_tf_cmd[0], sizeof(lidar_tf_cmd) / sizeof(struct cli_command));
}


